import socket
import time
# 096:093:094:093:100:090

udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# 使用套件字收发数据
# 目标IP 和端口，元组类型
ip_adders = ('192.168.137.2', 8008)
flag = False


def send(send_data):
    # 创建一个udp套件字
    udp_socket.sendto(send_data.encode('utf-8'), ip_adders)
    # 关闭套件字
    time.sleep(0.01)


def close_socket():
    udp_socket.close()


def accept():
    # 创建一个udp套件字
    udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    # 绑定本地相关信息，如果不绑定，则系统会随机分配，必须绑定本电脑的ip和port
    local_addr = ('192.168.137.1', 8010)  # 元组的第一个参数为本机IP，可以为空字符串，会自动生成本机IP
    udp_socket.bind(local_addr)
    # 等待接收方发送数据
    # rs中存储的是一个元组（接收到的数据，（发送方的ip，port））
    rs_data = udp_socket.recvfrom(1024)
    rs_masg = rs_data[0]
    # rs_addr = rs_data[1]
    # 接收到的数据解码展示
    # print(rs_masg.decode('utf-8'))
    return rs_masg.decode('utf-8')

    # 关闭套件字
    udp_socket.close()


def get_yaw(rs_msg):
    cont = 0
    yaw = ''
    for s in range(len(rs_msg)):
        if rs_msg[s] == ":":
            cont += 1
        if cont == 2:
            s += 1
            while rs_msg[s] != ":":
                yaw += rs_msg[s]
                s += 1
            return yaw


if __name__ == '__main__':
    send_data = "096:096:094:093:100:090"
    left = 96
    right = 90
    rs_data = accept()  # 接收数据
    begin_yaw = get_yaw(rs_data)  # 起始罗盘位置
    print("起始位置：" + begin_yaw)
    # "0"+left+":"+"0"+right+":094:093:100:090"
    send_data = "0" + str(left) + ":" + "0" + str(right) + ":094:093:100:090"
    #go_data= "0" + str(left) + ":" + "0" + str(right) + ":094:093:100:090"
    print(send_data)

    ins = input()
    if ins == "go":
        flag = True

# 扭转后 按下start键
    i = 0
    while True:
        i += 1
        send(send_data)  # 发送控制数据
        rs_data = accept()  # 接收数据
        now_yaw = get_yaw(rs_data)  # 起始罗盘位置
        print("现在位置：" + now_yaw)
        if i % 5 == 0:
            if int(now_yaw) - int(begin_yaw) < 0:  # 右转
                left = 95
                right = 95
                if int(now_yaw) - int(begin_yaw) < 5:
                    left = 96
                    right = 96
            elif int(now_yaw) - int(begin_yaw) > 0:  # 左转
                left = 90
                right = 90
                if int(now_yaw) - int(begin_yaw) > 5:
                    left = 89
                    right = 89
            else:
                left = 96
                right = 90
        print(left, " ", right)
        left_data = str(left)
        right_data = str(right)
        send_data = "0" + str(left) + ":" + "0" + \
            str(right) + ":094:093:100:090"
        print(send_data)
        print("############################")
